/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Victor;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    private Victor leftBack = new Victor(1);
    private Victor leftFront = new Victor(2);
    private Victor rightBack = new Victor(3);
    private Victor rightFront = new Victor(4);
    private double curve = .3;
    private double deadband = .1;
    private double leftSpeed;
    private double rightSpeed;
    private Joystick leftStick = new Joystick(1);
    private Joystick rightStick = new Joystick(2);
    double autonomousSpeed = .5;

    public void robotInit() {

        System.out.println("Hi! I'm the robot.I just thought I'd let you know - your code has finished builing.");

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {

        setLeftSpeed(leftStick.getY());
        setRightSpeed(rightStick.getY());

    }

    public void setRightVictors() {
        rightFront.set(rightSpeed);
        rightBack.set(rightSpeed);
        setRightVictors();
    }

    public void setLeftVictors() {
        leftFront.set(leftSpeed);
        leftBack.set(leftSpeed);
    }

    public void setLeftSpeed(double leftSpeed) {

        if (leftStick.getY() > deadband) {
            leftSpeed = leftStick.getY() * curve;
            setLeftVictors();
        }

    }

    public void setRightSpeed(double rightSpeed) {

        if (rightStick.getY() > deadband) {
            rightSpeed = rightStick.getY() * curve;
        }
    }
//autonomous specific methods
    public void spinRight(double autonomousSpeed) {
        setLeftSpeed(-autonomousSpeed);
        setRightSpeed(autonomousSpeed);
    }

    public void spinLeft(double autonomousSpeed) {
        setLeftSpeed(autonomousSpeed);
        setRightSpeed(-autonomousSpeed);
    }

    public void Forward() {
        setLeftSpeed(autonomousSpeed);
        setRightSpeed(autonomousSpeed);

    }
}
